//TURTLEMAP
// Does some stuff necessary to generate a map from scans, odometry and transformations

#include "scan_node.hpp"
#include "mapstore.hpp"
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "tf/tf.h"
#include "tf/transform_listener.h"  
#include <vector>
#include <deque>

class TurtleMap {

public:

    static const float MIN_DISTANCE = 0.1;
    static const float MIN_YAW_DIFF = M_PI_4/7.0 + 0.12;
    static const int SCANS_TO_USE = 4;

    ros::Subscriber laserSub;
    tf::TransformListener listener;
    ros::NodeHandle handle;
    Mapstore mapStore;
    std::vector<Scan_node> nodes; //Needs edges later, right now it's OK.

    TurtleMap(ros::NodeHandle& nh) : listener(), mapStore(nh) {
        handle = nh;
        laserSub = nh.subscribe("base_scan", 1, &TurtleMap::laserCallback, this);
        Scan_node::setResolution(0.05); //Default value, might need adjustment
        Scan_node::setMaxRange(3.5);

        mapStore.nodeVecPtr = &nodes;

        ros::spinOnce();
    }

    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
        ROS_INFO("laserCallback called");
        tf::StampedTransform robotRelativeToTrajectory;
        geometry_msgs::TransformStamped roboTransform;

        static std::deque<sensor_msgs::LaserScan> latestScans;

        latestScans.push_back(*msg);
        if(latestScans.size() > SCANS_TO_USE) {
            latestScans.pop_front();
        }

        try {
            listener.waitForTransform("map", "base_link", ros::Time::now(), ros::Duration(3.0));
            listener.lookupTransform("map", "base_link", ros::Time::now(), robotRelativeToTrajectory);
            tf::transformStampedTFToMsg(robotRelativeToTrajectory, roboTransform);

            float distance = MIN_DISTANCE+1;
            float yawDiff = MIN_YAW_DIFF+1;
            if(nodes.size() > 0) {
                distance = fast_distance(nodes.back().getTransform().transform.translation.x, nodes.back().getTransform().transform.translation.y, roboTransform.transform.translation.x, roboTransform.transform.translation.y);
                yawDiff = fabs(tf::getYaw(nodes.back().getTransform().transform.rotation)-tf::getYaw(roboTransform.transform.rotation));
            }
            if(distance > MIN_DISTANCE || yawDiff > MIN_YAW_DIFF) {

                Scan_node node;
                nodes.push_back(node);
                ROS_INFO("Node created.");
                ROS_INFO("Pushed into vector...");
                node.storePtr = &mapStore;
                ROS_INFO("Callbacks set.");
                ROS_INFO("Setting transform...");
                node.setTransform(roboTransform);
                ROS_INFO("Transform set. Setting scan...");
                for(int i=0; i<latestScans.size(); i++) {
                    node.addScan(latestScans[i], (i==latestScans.size()-1));
                }
                //node.updateRender();
                ROS_INFO("Scan set.");
                //mapStore.generateMap(nodes);
                ROS_INFO("Map generated.");
                latestScans.clear();
            }

        }
        catch (const tf::TransformException & ex) {
            ROS_ERROR("TF error in TurtleMap::laserCallBack: %s", ex.what());
        }
    }

    void spin() {
        ros::Rate rate(0.5);
        while(ros::ok()) {

            mapStore.publishMap();
            ros::spinOnce();
            rate.sleep();
        }
    }

    float fastSqrt(float number) {

        //From Quake 3, comments left in respecting the original.

        long i;
        float x2, y;
        const float threehalfs = 1.5F;

        x2 = number * 0.5F;
        y  = number;
        i  = * ( long * ) &y;  // evil floating point bit level hacking
        i  = 0x5f3759df - ( i >> 1 ); // what the fuck?
        y  = * ( float * ) &i;
        y  = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration

        return number*y;
    }

    float fast_distance(float x1, float y1, float x2, float y2)
    {
        return fastSqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
    }
};

int main(int argc, char ** argv) {
    std::cout << "Turtlemap node launch..." << std::endl;
    ros::init(argc, argv, "turtlemap"); // Initiate new ROS node named "turtlemap"
    ros::NodeHandle n;
    std::cout << "Creating mapper..." << std::endl;
    TurtleMap mapper(n); // Create new mapper
    std::cout << "Mapper created." << std::endl;
    mapper.spin(); // Start loop
    std::cout << "Spin started" << std::endl;
    return 0;
}
